% EKF-SLAM仿真主程序（激光雷达版）

clear all;
close all;
clc;

% 添加路径
addpath(fullfile(fileparts(mfilename('fullpath')), '..', 'common'));
addpath(fullfile(fileparts(mfilename('fullpath')), '..', 'occupancy_grid'));

% 轨迹选择
fprintf('========================================\n');
fprintf('  EKF-SLAM 轨迹模式选择\n');
fprintf('========================================\n');
fprintf('1. 固定路径点轨迹（可重复，适合对比）\n');
fprintf('2. 动态避障轨迹（随机探索）\n');
fprintf('========================================\n');
choice = input('请选择轨迹模式 [1/2] (默认1): ', 's');
if isempty(choice)
    choice = '1';
end

%% 加载统一配置
config = get_slam_config();

% 应用选择
if choice == '1'
    config.use_fixed_trajectory = true;
    fprintf('使用固定路径点轨迹\n\n');
elseif choice == '2'
    config.use_fixed_trajectory = false;
    fprintf('使用动态避障轨迹\n\n');
else
    fprintf('无效选择，使用默认（固定路径点）\n\n');
    config.use_fixed_trajectory = true;
end

% 解包常用参数
dt = config.dt;
total_time = config.total_time;
steps = config.steps;
Q = config.Q;
R = config.R;
laser_range = config.laser_range;
laser_fov = config.laser_fov;
num_beams = config.num_beams;
laser_noise = config.laser_noise;

% 统一环境地图
[~, obstacles] = generate_landmarks();
n_obstacles = size(obstacles, 1);

%% 初始化状态
if config.use_fixed_trajectory
    true_state = config.initial_state;
else
    % 动态模式：使用随机初始方向
    initial_theta = 0;  % 固定初始方向：水平向右
    true_state = [config.initial_state(1); config.initial_state(2); initial_theta];
end
state = true_state;
P = config.initial_P;

% 轨迹配置
USE_FIXED_TRAJECTORY = config.use_fixed_trajectory;

if USE_FIXED_TRAJECTORY
    fprintf('EKF-SLAM + 激光雷达 [固定路径点轨迹, 障碍物:%d, 时长:%.0fs]\n', n_obstacles, total_time);
    controls = generate_waypoint_trajectory(config.waypoints, dt, total_time, true_state);
else
    fprintf('EKF-SLAM + 激光雷达 [随机轨迹, 障碍物:%d, 时长:%.0fs]\n', n_obstacles, total_time);
    v_base = config.v_base;
    omega_base = config.omega_base;
end

% 路标管理
landmark_map = containers.Map('KeyType', 'int32', 'ValueType', 'any');

% 数据存储
true_trajectory = zeros(3, steps);
estimated_trajectory = zeros(3, steps);

% 运动控制状态（仅在非固定轨迹模式下使用）
if ~USE_FIXED_TRAJECTORY
    motion_state = struct();
    motion_state.boundary_avoiding = false;
    motion_state.obstacle_avoiding = false;
    motion_state.avoidance_timer = 0;
    motion_state.angle_diff = 0;
    motion_state.v = config.v_base;
    motion_state.omega = config.omega_base;
end

%% 主循环
figure('Position', config.display.figure_position);

for t = 1:steps
    %% 获取控制输入
    if USE_FIXED_TRAJECTORY
        % 使用固定轨迹
        control = controls(:, t);
    else
        % 使用安全运动控制
        [v, omega, motion_state] = safe_motion_controller(true_state, obstacles, motion_state, v_base, omega_base, dt);
        control = [v; omega];
    end
    
    %% 真实运动
    true_state = motion_model(true_state, control, dt, Q);
    
    % 安全检查
    [still_collision, ~] = check_collision(true_state, obstacles, 0.5);
    if still_collision && t > 1
        true_state = true_trajectory(:, t-1);
        if ~USE_FIXED_TRAJECTORY
            motion_state.omega = 0.8 * sign(randn());
            motion_state.obstacle_avoiding = true;
            motion_state.avoidance_timer = 20;
        end
    end
    
    true_trajectory(:, t) = true_state;
    
    %% EKF预测
    [state, P] = ekf_predict(state, P, control, dt, Q);
    
    %% 激光观测
    [observations, ~] = observation_model_laser(true_state, obstacles, ...
                                    laser_range, laser_fov, num_beams, laser_noise);
    
    %% 数据关联与更新
    if ~isempty(observations)
        % 提取已知路标信息
        n_landmarks = (length(state) - 3) / 2;
        landmarks = [];
        landmark_ids = [];
        P_landmarks = {};
        
        if n_landmarks > 0
            keys = landmark_map.keys();
            for k = 1:length(keys)
                lm_id = keys{k};
                lm_idx = landmark_map(lm_id);
                m_idx = 3 + (lm_idx - 1) * 2;
                landmarks = [landmarks; state(m_idx+1:m_idx+2)'];
                landmark_ids = [landmark_ids; lm_id];
                % 提取路标协方差
                P_lm = P(m_idx+1:m_idx+2, m_idx+1:m_idx+2);
                P_landmarks{end+1} = P_lm;
            end
        end
        
        % 数据关联（使用自适应阈值）
        [associations, is_new] = data_association(observations, state(1:3), ...
                                                  landmarks, landmark_ids, P_landmarks, R, []);
        
        % 处理每个观测
        for i = 1:size(observations, 2)
            z = observations(:, i);
            
            if is_new(i)
                % 新路标：初始化
                [state, P] = initialize_landmark(state, P, z, R);
                % 分配新ID（使用全局计数器）
                new_id = landmark_map.Count + 1;
                landmark_map(new_id) = (length(state) - 3) / 2;
            else
                % 已知路标：更新
                assoc_idx = associations(i);
                lm_id = landmark_ids(assoc_idx);
                landmark_idx = landmark_map(lm_id);
                [state, P] = ekf_update(state, P, z, landmark_idx, R);
            end
        end
    end
    
    estimated_trajectory(:, t) = state(1:3);
    
    %% 可视化
    if mod(t, config.display.update_interval) == 0
        plot_slam_laser(true_state, state, P, true_trajectory(:, 1:t), ...
                       estimated_trajectory(:, 1:t), obstacles, landmark_map, t*dt, 'EKF-SLAM');
        drawnow;
    end
end

%% 结果统计
fprintf('\n仿真完成\n');
fprintf('提取特征数: %d\n', landmark_map.Count);

position_error = sqrt(sum((true_trajectory(1:2, :) - estimated_trajectory(1:2, :)).^2, 1));
fprintf('平均误差: %.3f m\n', mean(position_error));
fprintf('最大误差: %.3f m\n', max(position_error));

save('ekf_slam_result.mat', 'true_trajectory', 'estimated_trajectory', 'position_error', 'landmark_map');
